
#ifndef ISPARSESDF_H
#define ISPARSESDF_H

#include <BBody>
#include "IGjkEpaSolver.h"

namespace BWE
{

#define Bet16Bits(d) ((((BUInt)(((const BByte*)(d))[1])) << 8) + (BUInt)(((const BByte*)(d))[0]))

	struct ISparseSdf
	{
		struct IntFrac
		{
			int b;
			int i;
			BReal f;
		};
		struct Cell
		{
			BReal d[4][4][4];
			int c[3];
			int puid;
			unsigned hash;
			BShape* pclient;
			Cell* next;
		};

		BArray<Cell*> cells;
		BReal voxelsz;
		BReal m_defaultVoxelsz;
		int puid;
		int ncells;
		int m_clampCells;
		int nprobes;
		int nqueries;

		static inline BReal distanceToShape(const BVector& x, BShape* shape)
		{
			BMatrix unit;
			IGjkEpaSolver::Result result;
			return (IGjkEpaSolver::SignedDistance(x, 0, shape, unit, result));
		}

		static inline IntFrac decompose(BReal x)
		{
			IntFrac r;
			x /= 3;
			const int o = x < 0 ? (int)(-x + 1) : 0;
			x += o;
			r.b = (int)x;
			const BReal k = (x - r.b) * 3;
			r.i = (int)k;
			r.f = k - r.i;
			r.b -= o;
			return (r);
		}

		static inline BReal lerp(BReal a, BReal b, BReal t)
		{
			return (a + (b - a) * t);
		}

		static inline BUInt hash(int x, int y, int z, const BShape* shape)
		{
			struct MySet
			{
				int x = 0;
				int y = 0;
				int z = 0;
				int w = 0;
				void* p = 0;
			};

			MySet myset;
			myset.x = x;
			myset.y = y;
			myset.z = z;
			myset.w = 0;
			myset.p = (void*)shape;
			const char* ptr = (const char*)&myset;

			BUInt result = makeHash(ptr, sizeof(MySet));

			return result;
		}

		static inline BUInt makeHash(const char* data, int len)
		{
			BUInt hash = len, tmp;
			len >>= 2;

			for (; len > 0; len--)
			{
				hash += Bet16Bits(data);
				tmp = (Bet16Bits(data + 2) << 11) ^ hash;
				hash = (hash << 16) ^ tmp;
				data += 2 * sizeof(BUShort);
				hash += hash >> 11;
			}

			hash ^= hash << 3;
			hash += hash >> 5;
			hash ^= hash << 4;
			hash += hash >> 17;
			hash ^= hash << 25;
			hash += hash >> 6;

			return hash;
		}

		ISparseSdf()
		{
			m_clampCells = 256 * 1024;
			m_defaultVoxelsz = 0.25;
			cells.append((Cell*)0, 2383);
			reset();
		}
		~ISparseSdf()
		{
			reset();
		}

		void setDefaultVoxelsz(BReal sz)
		{
			m_defaultVoxelsz = sz;
		}

		void reset()
		{
			for (int i = 0, ni = cells.size(); i < ni; ++i)
			{
				Cell* pc = cells[i];
				cells[i] = 0;
				while (pc)
				{
					Cell* pn = pc->next;
					delete pc;
					pc = pn;
				}
			}
			voxelsz = m_defaultVoxelsz;
			puid = 0;
			ncells = 0;
			nprobes = 1;
			nqueries = 1;
		}

		void garbageCollect(int lifetime = 256)
		{
			const int life = puid - lifetime;
			for (int i = 0; i < cells.size(); ++i)
			{
				Cell*& root = cells[i];
				Cell* pp = 0;
				Cell* pc = root;
				while (pc)
				{
					Cell* pn = pc->next;
					if (pc->puid < life)
					{
						if (pp)
							pp->next = pn;
						else
							root = pn;
						delete pc;
						pc = pp;
						--ncells;
					}
					pp = pc;
					pc = pn;
				}
			}
			nqueries = 1;
			nprobes = 1;
			++puid;
		}

		int removeReferences(BShape* pcs)
		{
			int refcount = 0;
			for (int i = 0; i < cells.size(); ++i)
			{
				Cell*& root = cells[i];
				Cell* pp = 0;
				Cell* pc = root;
				while (pc)
				{
					Cell* pn = pc->next;
					if (pc->pclient == pcs)
					{
						if (pp)
							pp->next = pn;
						else
							root = pn;
						delete pc;
						pc = pp;
						++refcount;
					}
					pp = pc;
					pc = pn;
				}
			}
			return (refcount);
		}

		BReal evaluate(const BVector& x, BShape* shape, BVector& normal, BReal margin)
		{
			const BVector scx = x / voxelsz;
			const IntFrac ix = decompose(scx.x());
			const IntFrac iy = decompose(scx.y());
			const IntFrac iz = decompose(scx.z());
			const unsigned h = hash(ix.b, iy.b, iz.b, shape);
			int index = (int)(h % cells.size());
			Cell* root = cells[index];
			Cell* c = root;
			++nqueries;
			while (c)
			{
				++nprobes;
				if ((c->hash == h) &&
					(c->c[0] == ix.b) &&
					(c->c[1] == iy.b) &&
					(c->c[2] == iz.b) &&
					(c->pclient == shape))
				{
					break;
				}
				else
				{
					c = c->next;
				}
			}
			if (!c)
			{
				++nprobes;
				++ncells;
				if (ncells > m_clampCells)
				{
					reset();
				}
				c = new Cell();
				c->next = root;
				cells[index] = c;
				c->pclient = shape;
				c->hash = h;
				c->c[0] = ix.b;
				c->c[1] = iy.b;
				c->c[2] = iz.b;
				buildCell(*c);
			}
			c->puid = puid;
			/* Extract infos		*/
			const int o[] = { ix.i, iy.i, iz.i };
			const BReal d[] = { c->d[o[0] + 0][o[1] + 0][o[2] + 0],
								  c->d[o[0] + 1][o[1] + 0][o[2] + 0],
								  c->d[o[0] + 1][o[1] + 1][o[2] + 0],
								  c->d[o[0] + 0][o[1] + 1][o[2] + 0],
								  c->d[o[0] + 0][o[1] + 0][o[2] + 1],
								  c->d[o[0] + 1][o[1] + 0][o[2] + 1],
								  c->d[o[0] + 1][o[1] + 1][o[2] + 1],
								  c->d[o[0] + 0][o[1] + 1][o[2] + 1] };
			/* Normal	*/

			const BReal gx[] = { d[1] - d[0], d[2] - d[3], d[5] - d[4], d[6] - d[7] };
			const BReal gy[] = { d[3] - d[0], d[2] - d[1], d[7] - d[4], d[6] - d[5] };
			const BReal gz[] = { d[4] - d[0], d[5] - d[1], d[7] - d[3], d[6] - d[2] };
			normal.x() = lerp(lerp(gx[0], gx[1], iy.f), lerp(gx[2], gx[3], iy.f), iz.f);
			normal.y() = lerp(lerp(gy[0], gy[1], ix.f), lerp(gy[2], gy[3], ix.f), iz.f);
			normal.z() = lerp(lerp(gz[0], gz[1], ix.f), lerp(gz[2], gz[3], ix.f), iy.f);
			normal.normalize();

			/* Distance	*/
			const BReal d0 = lerp(lerp(d[0], d[1], ix.f), lerp(d[3], d[2], ix.f), iy.f);
			const BReal d1 = lerp(lerp(d[4], d[5], ix.f), lerp(d[7], d[6], ix.f), iy.f);
			return (lerp(d0, d1, iz.f) - margin);
		}

		void buildCell(Cell& cell)
		{
			const BVector org = BVector((BReal)cell.c[0], (BReal)cell.c[1], (BReal)cell.c[2]) * 3 * voxelsz;
			for (int k = 0; k <= 3; ++k)
			{
				const BReal z = voxelsz * k + org.z();
				for (int j = 0; j <= 3; ++j)
				{
					const BReal y = voxelsz * j + org.y();
					for (int i = 0; i <= 3; ++i)
					{
						const BReal x = voxelsz * i + org.x();
						cell.d[i][j][k] = distanceToShape(BVector(x, y, z), cell.pclient);
					}
				}
			}
		}

	};
}

#endif
